#include "PoseUpdater.h"

PoseUpdater::PoseUpdater()
{
}

PoseUpdater::~PoseUpdater()
{
}

void PoseUpdater::pose_cb(const gazebo_msgs::ModelStates &msg){
    int i, smart_index;
    std::vector<std::string> model_names = msg.name;
    //gazebo世界中有很多物品，每个物品都有自己的名字
    for(i=0; i<model_names.size(); i++){
        //找到我们机器人的名字并记录在vector中的下标
        if(model_names[i] == "smart"){
            smart_index = i;
            break;
        }
    }
    //如果没有smart这个机器人，则不往下执行
    if(i == model_names.size()) return;
    //从gazebo中获取位姿和速度信息
    geometry_msgs::Pose vehicle_position = msg.pose[smart_index];
	geometry_msgs::Twist vehicle_velocity = msg.twist[smart_index];
    geometry_msgs::Quaternion orientation = vehicle_position.orientation;
    tf::Quaternion RQ2;  
    double roll,pitch,yaw;
    //输入四元数，转化成欧拉角数在终端输出
    tf::quaternionMsgToTF(orientation,RQ2); 
    //将四元数转化为欧拉角
    tf::Matrix3x3(RQ2).getRPY(roll,pitch,yaw); 
    //记录时间戳
    ros::Time time_stamp = ros::Time::now();

    //车辆中心位姿
    geometry_msgs::PoseStamped center_pose = geometry_msgs::PoseStamped();
    center_pose.header.frame_id = "/world";
    center_pose.header.stamp = time_stamp;
    center_pose.pose.position = vehicle_position.position;
    center_pose.pose.orientation = vehicle_position.orientation;
    this->center_pose_pub.publish(center_pose);

    //车辆后轴位姿
	geometry_msgs::PoseStamped rear_pose = geometry_msgs::PoseStamped();
    center_pose.header.frame_id = "/world";
    center_pose.header.stamp = time_stamp;
    double center_x = vehicle_position.position.x;
    double center_y = vehicle_position.position.y;
    double rear_x = center_x - cos(yaw) * 1.0058;
    double rear_y = center_y - sin(yaw) * 1.0058;
    rear_pose.pose.position.x = rear_x;
    rear_pose.pose.position.y = rear_y;
    rear_pose.pose.orientation = vehicle_position.orientation;
    this->rear_pose_pub.publish(rear_pose);

    //车辆速度信息
	geometry_msgs::TwistStamped velocity = geometry_msgs::TwistStamped();
    velocity.header.frame_id = "";
    velocity.header.stamp = time_stamp;
    velocity.twist.linear = vehicle_velocity.linear;
    velocity.twist.angular = vehicle_velocity.angular;
    this->vel_pub.publish(velocity);
    
}